#pragma once
#include <srrg_config/configurable.h>
#include <srrg_data_structures/correspondence.h>
#include <srrg_data_structures/platform.h>
#include <srrg_geometry/geometry_defs.h>
#include <srrg_messages/messages/base_sensor_message.h>
#include <srrg_property/property_container.h>
#include <srrg_viewer/drawable_base.h>

namespace srrg2_slam_interfaces {
  using namespace srrg2_core;

  // mother of correspondence finders
  // you need to assign it a correspondence vector
  // where to write the result before using it
  class CorrespondenceFinderBase : public srrg2_core::Configurable,
                                   public srrg2_core::PlatformUser,
                                   public srrg2_core::DrawableBase {
  public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    CorrespondenceFinderBase();

    //! key(s) for auxiliary data that can be produced/processed by subtypes
    //! - correspondences generated by the aligner unit
    constexpr static const char* auxiliary_data_suffix_correspondences = "_correspondences";
    //!  - correspondences generated by loop closure detection unit
    constexpr static const char* auxiliary_data_suffix_closure = "_loop_closure";

    const CorrespondenceVector* correspondences() const {
      return _correspondences;
    }

    void setCorrespondences(CorrespondenceVector* correspondences_) {
      _correspondences = correspondences_;
    }

    void setMeasurement(BaseSensorMessagePtr measurement_) {
      _measurement = measurement_;
    }

    virtual void compute() = 0;

  protected:
    CorrespondenceVector* _correspondences = 0;
    BaseSensorMessagePtr _measurement;
  };

  using CorrespondenceFinderBasePtr = std::shared_ptr<CorrespondenceFinderBase>;
  
  // typed correspondence finder
  template <typename TransformType_, typename FixedType_, typename MovingType_>
  class CorrespondenceFinder_ : public CorrespondenceFinderBase {
  public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    using TransformType = TransformType_;
    using MovingType    = MovingType_;
    using FixedType     = FixedType_;

    void setFixed(FixedType* fixed_) {
      _fixed              = fixed_;
      _fixed_changed_flag = true;
    }
    void setMoving(MovingType* moving_) {
      _moving              = moving_;
      _moving_changed_flag = true;
    }
    FixedType* fixed() {
      return _fixed;
    }
    MovingType* moving() {
      return _moving;
    }
    virtual ~CorrespondenceFinder_() {
    }
    virtual void setEstimate(const TransformType& est_) = 0;
    virtual void compute()                              = 0;

  protected:
    bool _fixed_changed_flag  = true;
    bool _moving_changed_flag = true;
    FixedType* _fixed         = nullptr;
    MovingType* _moving       = nullptr;
  };

} // namespace srrg2_slam_interfaces
